#include "pid.h"



/**
 * @brief 	绝对值限幅
 */
void abs_limit(float *a, float ABS_MAX){
    if(*a > ABS_MAX)
        *a = ABS_MAX;
    if(*a < -ABS_MAX)
        *a = -ABS_MAX;
}

/**
 * @brief 	全向轮pid专用计算函数,默认采用位置式pid
 * @param 	pid					pid参数结构体指针
 * @param	get_v				当前值
 * @param	target_v			目标值
 * @retval	输出值
 */
float chassis_pid_calc(pid_t* pid, float get_v, float target_v)
{
	if(pid == NULL)return 0.0f;
	//更新值
	pid->get[NOW] = get_v;
    pid->set[NOW] = target_v;
    pid->err[NOW] = target_v - get_v;
	
	//死区控制
	if (pid-> deadband != 0 && ABS(pid->err[NOW]) < pid->deadband) 
	{
		//清除积分
		pid-> integral = 0.0f;
		return 0;
	}
	
	
	//pid计算
	/*	i项	*/
	//变速积分
	pid-> integral	+=	(pid->err[NOW]);
	//积分限幅 
	abs_limit(&(pid-> integral), pid-> IntegralLimit);
	pid-> iout	=	pid->i * pid->integral;
	
	/*	d项	*/
	//不微分先行	(基于测量值变化率)
	float derivative	=	pid-> err[NOW] - pid-> err[LAST];
	pid-> dout	=	pid-> d * derivative; 		//(此处将pid控制周期也算入到参数d里面)

	/*	p项	*/
	pid-> pout	=	pid-> p * pid-> err[NOW];		//使用滤波后的误差计算p项
	
	/*	前馈计算	*/
	float feedforward = 5.0f;
	if(pid->f != 0)	//前馈系数不为0才计算前馈
	{
		feedforward = pid-> f *	(pid->set[NOW] - pid->set[LAST]);	//前馈项
		//前馈限幅
		abs_limit(&feedforward, pid->MaxOutput * 0.2f);		//前馈不超过最大输出的20%
	}
	
	/*	计算输出	*/
    pid->pos_out = pid->pout + pid->iout + pid->dout + feedforward;
	//输出限幅
    abs_limit(&(pid->pos_out), pid->MaxOutput);
	//更新上次输出
    pid->last_pos_out = pid->pos_out;

	//更新数据
	pid->err[LLAST] = pid->err[LAST];    pid->err[LAST] = pid->err[NOW];
    pid->get[LLAST] = pid->get[LAST];    pid->get[LAST] = pid->get[NOW];
    pid->set[LLAST] = pid->set[LAST];    pid->set[LAST] = pid->set[NOW];

	
	return pid->pos_out;
}

/**
 * @brief 	pid计算函数
 * @param 	pid					pid参数结构体指针
 * @param	get_v				当前值
 * @param	target_v			目标值
 * @retval	输出值
 */
float pid_calc(pid_t* pid, float get_v, float target_v)
{
	if(pid == NULL)return 0.0f;
	
	//更新值
	pid->get[NOW] = get_v;
    pid->set[NOW] = target_v;
    pid->err[NOW] = target_v - get_v;
	
	#if 0
	//误差过大，不进行输出
	if (pid->max_err != 0 && ABS(pid->err[NOW]) >  pid->max_err ) {
		return 0;
	}
	#endif
	//
	
	//死区控制
	if (pid->deadband != 0 && ABS(pid->err[NOW]) < pid->deadband) {
		//清除积分
		pid-> integral = 0.0f;
		return 0;
	}
	
	
	//计算
	/*	位置式	*/
	if(pid->pid_mode == POSITION_PID) 
    {
		//计算积分
		pid-> integral += pid-> err[NOW];
		//积分限幅 
		abs_limit(&(pid-> integral), pid-> IntegralLimit);
		
		//微分先行选项
		float derivate = (pid->d_first_mode == D_FIRST_EN)?
					pid->set[NOW] - pid->set[LAST] : pid->err[NOW] - pid->err[LAST] ;
		
		//计算p、i、d三项的输出
        pid->pout = pid-> p * pid->err[NOW];
        pid->iout = pid-> i * pid-> integral;
        pid->dout = pid-> d * derivate;
		
		//计算前馈
		/*	前馈计算	*/
		float feedforward = 0.0f;
		if(pid->f != 0)	//前馈系数不为0才计算前馈
		{
			feedforward = pid-> f *	(pid->set[NOW] - pid->set[LAST]);	//前馈项
			//前馈限幅
			abs_limit(&feedforward, pid->MaxOutput * 0.2f);		//前馈不超过最大输出的20%
		}
		
		//计算输出
        pid->pos_out = pid->pout + pid->iout + pid->dout + feedforward;
		//输出限幅
        abs_limit(&(pid->pos_out), pid->MaxOutput);
		//更新上次输出
        pid->last_pos_out = pid->pos_out;
    }
	
	#if 0		//目前不用,注释掉
	/*	增量式	*/
    else if(pid->pid_mode == DELTA_PID) 
    {
		//计算p、i、d三项的输出
        pid->pout = pid->p * (pid->err[NOW] - pid->err[LAST]);
        pid->iout = pid->i * pid->err[NOW];
        pid->dout = pid->d * (pid->err[NOW] - 2*pid->err[LAST] + pid->err[LLAST]);
        //进行积分限幅和输出限幅，得到输出值
        abs_limit(&(pid->iout), pid->IntegralLimit);
        pid->delta_u = pid->pout + pid->iout + pid->dout;
        pid->delta_out = pid->last_delta_out + pid->delta_u;
        abs_limit(&(pid->delta_out), pid->MaxOutput);
		//更新上次输出
        pid->last_delta_out = pid->delta_out;	
    }
	#endif
	//
	
	//更新数据
	pid->err[LLAST] = pid->err[LAST];
    pid->err[LAST] = pid->err[NOW];
	
    pid->get[LLAST] = pid->get[LAST];
    pid->get[LAST] = pid->get[NOW];
	
    pid->set[LLAST] = pid->set[LAST];
    pid->set[LAST] = pid->set[NOW];
	
	//返回计算值
	return pid->pid_mode==POSITION_PID ? pid->pos_out : pid->delta_out;
}


/**
 * @brief 	初始化pid参数
 * @param 	pid					需要设置参数的pid参数结构体指针
 * @param 	mode				pid模式			@args POSITION_PID （位置式）或 DELTA_PID（增量式）
 * @param	d_first_mode		微分先行模式	@args D_FIRST_EN (使能)	或 D_FIRST_DISEN (禁止)
 * @param 	maxout				输出限幅值
 * @param 	intergral_limit		积分限幅值
 * @param 	kp, ki, kd, kf		pid参数
 */
void PID_struct_init(pid_t* pid,uint32_t mode, uint32_t d_first_mode, 
						uint32_t maxout,uint32_t intergral_limit,
							float deadband, float kp, float ki, float kd, float kf)
{
	pid->IntegralLimit = intergral_limit;
    pid->MaxOutput = maxout;
    pid->pid_mode = mode;
    pid->p = kp;
    pid->i = ki;
    pid->d = kd;
	pid->f = kf; 
	pid->deadband = deadband;
	pid->d_first_mode = d_first_mode;
}

/**
 * @brief 	中途修改pid参数
 * @param 	kp, ki, kd, kf			pid参数
 */
static void pid_reset(pid_t	*pid, float kp, float ki, float kd, float kf)
{
    pid->p = kp;
    pid->i = ki;
    pid->d = kd;
	pid->f = kf;
}


